#include <path_planning_study/global_planning_node.h>


// 初始化算法
void GlobalPlanning::InitPlanning(){
    // 开始规划路径
    switch(__path_planning_type){
        case PathPlanningType::A_STAR:
            cout << "--> [InitPlanning] A_STAR......" << endl;
            AstarInit();
            break;
        case PathPlanningType::JPS:
            cout << "--> [InitPlanning] JPS......" << endl;
            JPSInit();
            break;
        default:
            ROS_ERROR("Unknown path planning type");
            break;
    } 
}

// 设置地图数据
void GlobalPlanning::SetMap(){
    // 开始规划路径
    switch(__path_planning_type){
        case PathPlanningType::A_STAR:
            cout << "--> [SetMap] A_STAR......" << endl;
            SetAstarMap(__map_image);
            break;
        case PathPlanningType::JPS:
            cout << "--> [SetMap] JPS......" << endl;
            SetJPSMap(__map_image);
            break;
        default:
            ROS_ERROR("Unknown path planning type");
            break;
    } 
}

// geometry_msgs::Pose -> cv::Point
cv::Point GlobalPlanning::Pose2CvPoint(geometry_msgs::Pose pose){
    return __map_helper.World2OpencvPixel(
        cv::Point2f(pose.position.x, pose.position.y),
        cv::Point2f(__map_ptr->info.origin.position.x, __map_ptr->info.origin.position.y),
        __map_ptr->info.resolution, 
        __map_ptr->info.height
    );
}

// cv::Point -> geometry_msgs::Pose
geometry_msgs::Pose GlobalPlanning::CvPoint2Pose(cv::Point point){
    geometry_msgs::Pose pose;
    cv::Point2f point_2f = __map_helper.OpencvPixel2World(
            point, 
            cv::Point2f(__map_ptr->info.origin.position.x, __map_ptr->info.origin.position.y),
            __map_ptr->info.resolution, 
            __map_ptr->info.height
        );
    pose.position.x = point_2f.x;
    pose.position.y = point_2f.y;
    return pose;
}


// 发布原始路径信息
void GlobalPlanning::PublishPathOrigin(vector<cv::Point>& path_points){
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.header.stamp = ros::Time::now();
    for(auto& point : path_points){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header = path_msg.header;
        pose_stamp.pose = CvPoint2Pose(point);
        // cout << "--> [path_msg] point_2f (" << point_2f.x << ", " << point_2f.y << ")"<< endl;
        pose_stamp.pose.position.z = 0.05;
        path_msg.poses.emplace_back(pose_stamp);
    }
    __path_pub.publish(path_msg);
}


// 发布扩展点地图信息
void GlobalPlanning::PublishExpendPointsMap(vector<cv::Point>& expand_poinds){
    if(__expand_points_pub.getNumSubscribers() > 0){
        nav_msgs::OccupancyGrid grid_msg;
        grid_msg.header = __map_ptr->header;
        grid_msg.info = __map_ptr->info;
        grid_msg.data.assign(__map_ptr->data.size(), 0);
        for(auto& point : expand_poinds){
            long index = (__map_ptr->info.height - point.y) * __map_ptr->info.width + point.x;
            if(index < 0 || index >= __map_ptr->data.size())
                continue;
            grid_msg.data[index] = 30;
        }
        __expand_points_pub.publish(grid_msg);
    }
}